function [line1, line2] = cal_lines(q)
l1 = 1; l2 = 1;
line1.p1 = [0; 0; 0];
line1.p2 = [l1 * cos(q(1)); 0.0; l1 * sin(q(1))];
line2.p1 = [l1 * cos(q(1)); 0.0; l1 * sin(q(1))];
line2.p2 = [l1 * cos(q(1)) + l2 * cos(q(1) + q(2)); 
    0.0; 
    l1 * sin(q(1)) + l2 * sin(q(1) + q(2))];
